/**
 * Star recognizer using the CImg library and CCFits.
 *
 * Copyright (C) 2015 Carsten Schmitt
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "CImg.h"
#include "star_recognizer.h"
#include "elog.h"
#include "elog_file.h"

#include <assert.h>

#include <iostream>
#include <functional>
#include <list>
#include <set>
#include <array>
#include <vector>
#include <string>
 


using namespace std;
using namespace cimg_library;


typedef tuple<int /*x*/,int /*y*/> PixelPosT;
typedef set<PixelPosT> PixelPosSetT;
typedef list<PixelPosT> PixelPosListT;
 
typedef tuple<float, float> PixSubPosT;
// typedef tuple<float /*x1*/, float /*y1*/, float /*x2*/, float /*y2*/> FrameT;
 
struct StarInfoT {
  FrameT clusterFrame;
  FrameT cogFrame;
  FrameT hfdFrame;
  PixSubPosT cogCentroid;
  PixSubPosT subPixelInterpCentroid;
  float hfd;
  float fwhmHorz;
  float fwhmVert;
  float maxPixValue;
  bool saturated;
  float mean;
};
typedef list<StarInfoT> StarInfoListT;

/**
* Get all pixels inside a radius: http://stackoverflow.com/questions/14487322/get-all-pixel-array-inside-circle
* Algorithm: http://en.wikipedia.org/wiki/Midpoint_circle_algorithm
*/
bool
insideCircle(float inX /*pos of x*/, float inY /*pos of y*/, float inCenterX, float inCenterY, float inRadius)
{
  return (pow(inX - inCenterX, 2.0) + pow(inY - inCenterY, 2.0) <= pow(inRadius, 2.0));
}

int findBrightestStar(Mat& imgSrc, Rect& starMax, const char* path) {
	Mat imgGray;

	if (imgSrc.empty())
		return -1;

	if(imgSrc.channels() == 1)
		imgGray = imgSrc.clone();
	else
		cvtColor(imgSrc, imgGray, COLOR_RGB2GRAY);

	Scalar mean;  
    Scalar dev;
	meanStdDev(imgGray, mean, dev);
	float threshold = (mean.val[0] + 3.0 * dev.val[0]) > 254 ? 254 : (mean.val[0] + 3.0 * dev.val[0]);
	log_i("threshold:%f", threshold);

  	//Setup a threshold 
	cv::threshold(imgGray, imgGray, threshold, 0, THRESH_TOZERO);

	if(threshold < 254 ) {
		double scale = 255.0 / (255.0 - threshold - 1);
		double shift = -255.0 * (threshold + 1) / (255.0 - threshold - 1);
		imgGray.convertTo(imgGray, CV_8U, scale, shift);
	}

	//椭圆卷积核心，开运算，去除噪点，圆滑星点边界
	Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3), Point(-1, -1));
	morphologyEx(imgGray, imgGray, MORPH_OPEN, kernel);

	vector<vector<Point> > contours;
	findContours(imgGray, contours, noArray(), RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
	log_i("objects found:%d", (unsigned int)contours.size());

	Mat imgContour = Mat::zeros(imgGray.rows, imgGray.cols, CV_8UC3);
	drawContours(imgContour, contours, -1, Scalar(0,0,255));
	// imwrite(path, imgContour);
	// imwrite(path, imgSrc);

	// if(contours.size() > 20) {
	// 	log_i("objects found too much");
	// 	return -1;
	// }

	if(contours.size() < 1) {
		log_i("no objects found");
		return -1;
	}

	double areaMax = 0;
	for (int k = 0; k < contours.size(); k++) {
		Rect bRect = boundingRect(contours[k]);
		if(abs(bRect.width - bRect.height) >= 5)
			continue;

		double area = contourArea(contours[k]);
		if(area > areaMax) {
			areaMax = area;
			starMax = bRect;
		}

		rectangle(imgContour, bRect, Scalar(255, 255, 255), 1);
	}
	log_i("Max contour area:%f", areaMax);
	// imwrite(path, imgContour);
	// imwrite(path, imgSrc);

	if(areaMax > 2000) {
		log_i("star is too big");
		return -1;
	}

	if(areaMax < 400) {
		log_i("star is too small");
		return -1;
	}

	return 0;
}

int getImg(CImg<float> & inImg, MEDIA_BUFFER buf, recognizerType type, FrameT frame, const char* path)
{	
	int ret = 0;
	// cv::Mat src = cv::imread(filename, 1);
	// cv::Mat gray;
	
	// if (STAR_IMAGE == type) {
	// 	float x = std::get<0>(frame);
	// 	float y = std::get<1>(frame);
	// 	float w = std::get<2>(frame) - x;
	// 	float h = std::get<3>(frame) - y;
	// 	log_i("frame : %f, %f, %f, %f", std::get<0>(frame), std::get<1>(frame), std::get<2>(frame), std::get<3>(frame));
	// 	log_i("w = %d\n, h = %d", w, h);
	// 	if (w == 0 || h == 0) {
	// 		ret = -RECOGNIZER_NO_STAR;
	// 		return ret;
	// 	}
	// 	src = src(cv::Rect(x, y, w, h));
	// 	cv::imwrite(std::string("/userdata/Astro/1080p/maxMean_test/").append(to_string(index).append(".jpg")).c_str(), src);
	// }

	// cv::cvtColor(src, gray, COLOR_BGR2GRAY);

	// int width = src.cols;
	// int height = src.rows;                    
	// int channel = src.channels();
	// int depth = 1;

	// inImg.resize(width, height, 1, 1);

	// log_i("src width = %d, height = %d, channel = %d", width, height, channel);
																
	// cimg_forXY(inImg, x, y) {
	// 	inImg(x, y) = (float)gray.at<uchar>(y, x);                                                                                                                                                                                                                                                                                                         
	// }

	//从摄像头取图
	int width = 1920;
	int height = 1080;                                     
	
	if (buf  == NULL) {
		log_i("image buf error", buf);
		ret = -RECOGNIZER_IMAGE_ERROR;
		return ret;
	}

	cv::Mat src_bgr(height, width, CV_8UC3, (unsigned char*)RK_MPI_MB_GetPtr(buf));
	cv::Mat gray;
	cv::cvtColor(src_bgr, gray, cv::COLOR_BGR2GRAY);
	

	if (STAR_IMAGE == type) {
		float x = std::get<0>(frame);
		float y = std::get<1>(frame);
		float w = std::get<2>(frame) - x;
		float h = std::get<3>(frame) - y;
		width = w;
		height = h;
		log_i("frame : %f, %f, %f, %f", std::get<0>(frame), std::get<1>(frame), std::get<2>(frame), std::get<3>(frame));
		if (w == 0 || h == 0) {
			ret = -RECOGNIZER_NO_STAR;
			return ret;
		}
		gray = gray(cv::Rect(x, y, w, h));
		Mat save_src = src_bgr(Rect(x, y, w, h));
		// cv::imwrite(path, save_src);
	} else {
		log_i("frame : %f, %f, %f, %f", std::get<0>(frame), std::get<1>(frame), std::get<2>(frame), std::get<3>(frame));
	}

	inImg.resize(width, height, 1, 1);
	cimg_forXY(inImg, x, y) {
		inImg(x, y) = (float)gray.at<uchar>(y, x);
	}
	
	// RK_MPI_MB_ReleaseBuffer(buf);

	return 0;
} 
 
void thresholdOtsu(const CImg<float> & inImg, long inBitPix, CImg<float> * outBinImg) {
	CImg<> hist = inImg.get_histogram(pow(2.0, inBitPix));
	
	float sum = 0;
	cimg_forX(hist, pos) { sum += pos * hist[pos]; }
	
	float numPixels = inImg.width() * inImg.height();
	float sumB = 0, wB = 0, max = 0.0;
	float threshold1 = 0.0, threshold2 = 0.0;
	
	cimg_forX(hist, i) {
		wB += hist[i];
	
		if (! wB) { continue; }    
	
		float wF = numPixels - wB;
		
		if (! wF) { break; }
		
		sumB += i * hist[i];
	
		float mF = (sum - sumB) / wF;
		float mB = sumB / wB;
		float diff = mB - mF;
		float bw = wB * wF * pow(diff, 2.0);
		
		if (bw >= max) {
		threshold1 = i;
		if (bw > max) {
			threshold2 = i;
		}
		max = bw;            
		}
	} // end loop
	
	float th = (threshold1 + threshold2) / 2.0;
	
	*outBinImg = inImg; // Create a copy
	outBinImg->threshold(th); 
}
 
/**
 * Removes all white neighbours arond pixel from whitePixels
 * if they exist and adds them to pixelsToBeProcessed.
 */
void getAndRemoveNeighbours(PixelPosT inCurPixelPos, PixelPosSetT * inoutWhitePixels, PixelPosListT * inoutPixelsToBeProcessed) {
	const size_t _numPixels = 8, _x = 0, _y = 1;
	const int offsets[_numPixels][2] = { { -1, -1 }, { 0, -1 }, { 1, -1 },
										{ -1, 0 },              { 1, 0 },
										{ -1, 1 }, { 0, 1 }, { 1, 1 } };
	
	for (size_t p = 0; p < _numPixels; ++p) {
		PixelPosT curPixPos(std::get<0>(inCurPixelPos) + offsets[p][_x], std::get<1>(inCurPixelPos) + offsets[p][_y]);
		PixelPosSetT::iterator itPixPos = inoutWhitePixels->find(curPixPos);
	
		if (itPixPos != inoutWhitePixels->end()) {
		const PixelPosT & curPixPos = *itPixPos;
		inoutPixelsToBeProcessed->push_back(curPixPos);
		inoutWhitePixels->erase(itPixPos); // Remove white pixel from "white set" since it has been now processed
		}
	}
}
 
template<typename T> void
clusterStars(const CImg<T> & inImg, StarInfoListT * outStarInfos)
{
	PixelPosSetT whitePixels;
	
	cimg_forXY(inImg, x, y) {
		if (inImg(x, y)) {
		whitePixels.insert(whitePixels.end(), PixelPosT(x, y));
		}
	}
	
	// Iterate over white pixels as long as set is not empty
	while (whitePixels.size()) {
		PixelPosListT pixelsToBeProcessed;
	
		PixelPosSetT::iterator itWhitePixPos = whitePixels.begin();
		pixelsToBeProcessed.push_back(*itWhitePixPos);
		whitePixels.erase(itWhitePixPos);
	
		FrameT frame(inImg.width(), inImg.height(), 0, 0);
	
		while(! pixelsToBeProcessed.empty()) {
		PixelPosT curPixelPos = pixelsToBeProcessed.front();
	
		// Determine boundaries (min max in x and y directions)
		if (std::get<0>(curPixelPos) /*x*/ < std::get<0>(frame) /*x1*/) {    std::get<0>(frame) = std::get<0>(curPixelPos); }
		if (std::get<0>(curPixelPos) /*x*/ > std::get<2>(frame) /*x2*/) { std::get<2>(frame) = std::get<0>(curPixelPos); }
		if (std::get<1>(curPixelPos) /*y*/ < std::get<1>(frame) /*y1*/) {    std::get<1>(frame) = std::get<1>(curPixelPos); }
		if (std::get<1>(curPixelPos) /*y*/ > std::get<3>(frame) /*y2*/) { std::get<3>(frame) = std::get<1>(curPixelPos); }
	
		getAndRemoveNeighbours(curPixelPos, & whitePixels, & pixelsToBeProcessed);
		pixelsToBeProcessed.pop_front();
		}
	
		// Create new star-info and set cluster-frame.
		// NOTE: we may use new to avoid copy of StarInfoT...
		StarInfoT starInfo;
		starInfo.clusterFrame = frame;
		outStarInfos->push_back(starInfo);
	}
}
 
float calcIx2(const CImg<float> & img, int x) {
	float Ix = 0;
	cimg_forY(img, y) { Ix += pow(img(x, y), 2.0) * (float) x; }
	return Ix;
}
 
float calcJy2(const CImg<float> & img, int y) {
	float Iy = 0;
	cimg_forX(img, x) { Iy += pow(img(x, y), 2.0) * (float) y; }
	return Iy;
}
 
// Calculate Intensity Weighted Center (IWC)
void calcIntensityWeightedCenter(const CImg<float> & inImg, float * outX, float * outY) {
	assert(outX && outY);
	
	// Determine weighted centroid - See http://cdn.intechopen.com/pdfs-wm/26716.pdf
	float Imean2 = 0, Jmean2 = 0, Ixy2 = 0;
	
	for(size_t i = 0; i < inImg.width(); ++i) {
		Imean2 += calcIx2(inImg, i);
		cimg_forY(inImg, y) { Ixy2 += pow(inImg(i, y), 2.0); }
	}
	
	for(size_t i = 0; i < inImg.height(); ++i) {
		Jmean2 += calcJy2(inImg, i);
	}
	
	*outX = Imean2 / Ixy2;
	*outY = Jmean2 / Ixy2;
}
 
void calcSubPixelCenter(const CImg<float> & inImg, float * outX, float * outY, size_t inNumIter = 10 /*num iterations*/) {
	// Sub pixel interpolation
	float c, a1, a2, a3, a4, b1, b2, b3, b4;
	float a1n, a2n, a3n, a4n, b1n, b2n, b3n, b4n;
	
	assert(inImg.width() == 3 && inImg.height() == 3);
	
	b1 = inImg(0, 0); a2 = inImg(1, 0); b2 = inImg(2, 0);
	a1 = inImg(0, 1);  c = inImg(1, 1); a3 = inImg(2, 1);
	b4 = inImg(0, 2); a4 = inImg(1, 2); b3 = inImg(2, 2);
	
	for (size_t i = 0; i < inNumIter; ++i) {
		float c2 = 2 * c;
		float sp1 = (a1 + a2 + c2) / 4;
		float sp2 = (a2 + a3 + c2) / 4;
		float sp3 = (a3 + a4 + c2) / 4;
		float sp4 = (a4 + a1 + c2) / 4;
		
		// New maximum is center
		float newC = std::max({ sp1, sp2, sp3, sp4 });
		
		// Calc position of new center
		float ad = pow(2.0, -((float) i + 1));
	
		if (newC == sp1) {
		*outX = *outX - ad; // to the left
		*outY = *outY - ad; // to the top
	
		// Calculate new sub pixel values
		b1n = (a1 + a2 + 2 * b1) / 4;
		b2n = (c + b2 + 2 * a2) / 4;
		b3n = sp3;
		b4n = (b4 + c + 2 * a1) / 4;
		a1n = (b1n + c + 2 * a1) / 4;
		a2n = (b1n + c + 2 * a2) / 4;
		a3n = sp2;
		a4n = sp4;
	
		} else if (newC == sp2) {
		*outX = *outX + ad; // to the right
		*outY = *outY - ad; // to the top
	
		// Calculate new sub pixel values
		b1n = (2 * a2 + b1 + c) / 4;
		b2n = (2 * b2 + a3 + a2) / 4;
		b3n = (2 * a3 + b3 + c) / 4;
		b4n = sp4;
		a1n = sp1;
		a2n = (b2n + c + 2 * a2) / 4;
		a3n = (b2n + c + 2 * a3) / 4;
		a4n = sp3;
		} else if (newC == sp3) {
		*outX = *outX + ad; // to the right
		*outY = *outY + ad; // to the bottom
	
		// Calculate new sub pixel values
		b1n = sp1;
		b2n = (b2 + 2 * a3 + c) / 4;
		b3n = (2 * b3 + a3 + a4) / 4;
		b4n = (2 * a4 + b4 + c) / 4;
		a1n = sp4;
		a2n = sp2;
		a3n = (b3n + 2 * a3 + c) / 4;
		a4n = (b3n + 2 * a4 + c) / 4;
		} else {
		*outX = *outX - ad; // to the left
		*outY = *outY + ad; // to the bottom  
	
		// Calculate new sub pixel values
		b1n = (2 * a1 + b1 + c) / 4;
		b2n = sp2;
		b3n = (c + b3 + 2 * a4) / 4;
		b4n = (2 * b4 + a1 + a4) / 4;
		a1n = (b4n + 2 * a1 + c) / 4;
		a2n = sp1;
		a3n = sp3;
		a4n = (b4n + 2 * a4 + c) / 4;
		}
	
		c = newC; // Oi = Oi+1
	
		a1 = a1n;
		a2 = a2n;
		a3 = a3n;
		a4 = a4n;
	
		b1 = b1n;
		b2 = b2n;
		b3 = b3n;
		b4 = b4n;
	}
}
 
void calcCentroid(const CImg<float> & inImg, const FrameT & inFrame, PixSubPosT * outPixelPos, PixSubPosT * outSubPixelPos = 0, size_t inNumIterations = 10) {
	// Get frame sub img
	CImg<float> subImg = inImg.get_crop(std::get<0>(inFrame), std::get<1>(inFrame), std::get<2>(inFrame), std::get<3>(inFrame));
	
	float & xc = std::get<0>(*outPixelPos);
	float & yc = std::get<1>(*outPixelPos);
	
	// 1. Calculate the IWC
	calcIntensityWeightedCenter(subImg, & xc, & yc);
	
	if (outSubPixelPos) {
		// 2. Round to nearest integer and then iteratively improve.
		int xi = floor(xc + 0.5);
		int yi = floor(yc + 0.5);
	
		CImg<float> img3x3 = inImg.get_crop(xi - 1 /*x0*/, yi - 1 /*y0*/, xi + 1 /*x1*/, yi + 1 /*y1*/);
		
		// 3. Interpolate using sub-pixel algorithm
		float xsc = xi, ysc = yi;
		calcSubPixelCenter(img3x3, & xsc, & ysc, inNumIterations);
		
		std::get<0>(*outSubPixelPos) = xsc;
		std::get<1>(*outSubPixelPos) = ysc;
	}
}
 
 
/**
* Expects star centered in the middle of the image (in x and y) and mean background subtracted from image.
*
* HDF calculation: http://www005.upp.so-net.ne.jp/k_miyash/occ02/halffluxdiameter/halffluxdiameter_en.html
*                  http://www.cyanogen.com/help/maximdl/Half-Flux.htm
*
* NOTE: Currently the accuracy is limited by the insideCircle function (-> sub-pixel accuracy).
* NOTE: The HFD is estimated in case there is no flux (HFD ~ sqrt(2) * inOuterDiameter / 2).
* NOTE: The outer diameter is usually a value which depends on the properties of the optical
*       system and also on the seeing conditions. The HFD value calculated depends on this
*       outer diameter value.
*/
float calcHfd(const CImg<float> & inImage, unsigned int inOuterDiameter) {
	// Sum up all pixel values in whole circle
	float outerRadius = inOuterDiameter / 2;
	float sum = 0, sumDist = 0;
	int centerX = ceil(inImage.width() / 2.0);
	int centerY = ceil(inImage.height() / 2.0);
	
	cimg_forXY(inImage, x, y) {
		if (insideCircle(x, y, centerX, centerY, outerRadius)) {
		sum += inImage(x, y);
		sumDist += inImage(x, y) * sqrt(pow((float) x - (float) centerX, 2.0f) + pow((float) y - (float) centerY, 2.0f));
		}
	}
	// NOTE: Multiplying with 2 is required since actually just the HFR is calculated above
	return (sum ? 2.0 * sumDist / sum : sqrt(2.0) * outerRadius);
}
 
/**********************************************************************
* Helper classes
**********************************************************************/
struct DataPointT {
	float x;
	float y;
	DataPointT(float inX = 0, float inY = 0) : x(inX), y(inY) {}
};
  
typedef vector<DataPointT> DataPointsT;
  
struct GslMultiFitDataT {
	float y;
	float sigma;
	DataPointT pt;
};
  
typedef vector<GslMultiFitDataT> GslMultiFitParmsT;
  
typedef list<PixSubPosT> MyDataContainerT;
 
class MyDataAccessorT {
public:
  typedef MyDataContainerT TypeT;
  static DataPointT getDataPoint(size_t inIdx, TypeT::const_iterator inIt) {
    const PixSubPosT & pos = *inIt;
    DataPointT dp(get<0>(pos) /*inIdx*/, get<1>(pos) /*y*/);
    return dp;
  }
};
 
 
FrameT rectify(const FrameT & inFrame) {
    float border = 3;
    float border2 = 2.0 * border;
    float width = fabs(std::get<0>(inFrame) - std::get<2>(inFrame)) + border2;
    float height = fabs(std::get<1>(inFrame) - std::get<3>(inFrame)) + border2;
    float L = max(width, height);
    float x0 = std::get<0>(inFrame) - (fabs(width - L) / 2.0) - border;
    float y0 = std::get<1>(inFrame) - (fabs(height - L) / 2.0) - border;
    return FrameT(x0, y0, x0 + L, y0 + L);
}
 
recognizerResultT star_recognizer(MEDIA_BUFFER buf, int index, recognizerType type, FrameT frame, const char* path) {
    /* outerHfdDiameter depends on pixel size and focal length (and seeing...).
      Later we may calculate it automatically wihth goven focal length and pixel
      size of the camera. For now it is a "best guess" value.
    */
    const unsigned int outerHfdDiameter = 21;
    StarInfoListT starInfos;                          
    vector < list<StarInfoT *> > starBuckets;                            
    CImg<float> img;
    long bitPix = 8;
	recognizerResultT result;
	int maxStarCount = 5000;
	int minStarSize = 5;
	int maxStarSize = 200;
	int ret = 0;

    // Read file to CImg
    // std::string img_path("/userdata/Astro/");
    // std::string img_file = img_path.append(to_string(index)).append(".jpg");
    // log_i("img file path: %s", img_file.c_str());
    ret = getImg(img, buf, type, frame, path);
	if (ret == -RECOGNIZER_NO_STAR || ret == -RECOGNIZER_IMAGE_ERROR) {
		result.code = ret;
		return result;
	}
    
    // Create RGB image from fits file to paint boundaries and centroids (just for visualization)
    CImg<unsigned char> rgbImg(img.width(), img.height(), 1 /*depth*/, 3 /*3 channels - RGB*/);
    float min = img.min(), mm = img.max() - min;
    
    cimg_forXY(img, x, y) {
      int value = 255.0 * (img(x,y) - min) / mm;
      rgbImg(x, y, 0 /*red*/) = value;
      rgbImg(x, y, 1 /*green*/) = value;
      rgbImg(x, y, 2 /*blue*/) = value;
    }
  
    // rgbImg.save("../jpg_rgbImg.png");
    // AD noise reduction --> In: Loaded image, Out: Noise reduced image
    // NOTE: This step takes a while for big images... too long for usage in a loop ->
    //       Should only be used on image segments, later...
    //
    // http://cimg.sourceforge.net/reference/structcimg__library_1_1CImg.html
    CImg<float> & aiImg = img.blur_anisotropic(30.0f, /*amplitude*/
                          0.7f, /*sharpness*/
                          0.3f, /*anisotropy*/
                          0.6f, /*alpha*/
                          1.1f, /*sigma*/
                          0.8f, /*dl*/
                          30,   /*da*/
                          2,    /*gauss_prec*/
                          0,    /*interpolation_type*/
                          false /*fast_approx*/
                            );
    // Thresholding (Otsu) --> In: Noise reduced image, Out: binary image
    CImg<float> binImg;
    thresholdOtsu(aiImg, bitPix, & binImg);
    // Clustering --> In: binary image from thresholding, Out: List of detected stars, subimg-boundaries (x1,y1,x2,y2) for each star
    clusterStars(binImg, & starInfos);
 
	int recog_star_count = starInfos.size();
    log_i("recognizered %d stars", recog_star_count);

	if (0 == recog_star_count) {
		result.code = -RECOGNIZER_NO_STAR;
		return result;
	}
 
    // Calc brightness boundaries for possible focusing stars
    float maxPossiblePixValue = pow(2.0, bitPix) - 1;
    int maxMeanIndex = 0;
    float maxMean = 0.0;
    int i = 0;
	int max_w = 0;
	int maxWIndex = 0;
 
    // For each star
    for (StarInfoListT::iterator it = starInfos.begin(); it != starInfos.end(); ++it) {
        const FrameT & frame = it->clusterFrame;  
		FrameT & cogFrame = it->cogFrame;
        FrameT & hfdFrame = it->hfdFrame;
        PixSubPosT & cogCentroid = it->cogCentroid;
        PixSubPosT & subPixelInterpCentroid = it->subPixelInterpCentroid;
        float & hfd = it->hfd;
        float & fwhmHorz = it->fwhmHorz;
        float & fwhmVert = it->fwhmVert;
        float & maxPixValue = it->maxPixValue;
        bool & saturated = it->saturated;
        
        FrameT squareFrame = rectify(frame);
        
        // Centroid calculation --> In: Handle to full noise reduced image, subimg-boundaries (x1,y1,x2,y2), Out: (x,y) - abs. centroid coordinates
        calcCentroid(aiImg, squareFrame, & cogCentroid, & subPixelInterpCentroid, 10 /* num iterations */);
        std::get<0>(cogCentroid) += std::get<0>(squareFrame);
        std::get<1>(cogCentroid) += std::get<1>(squareFrame);
        std::get<0>(subPixelInterpCentroid) += std::get<0>(squareFrame);
        std::get<1>(subPixelInterpCentroid) += std::get<1>(squareFrame);
      
      
        // Calculate cog boundaries
        float maxClusterEdge = std::max(fabs(std::get<0>(frame) - std::get<2>(frame)), fabs(std::get<1>(frame) - std::get<3>(frame)));
        float cogHalfEdge = ceil(maxClusterEdge / 2.0);
        float cogX = std::get<0>(cogCentroid);
        float cogY = std::get<1>(cogCentroid);
        std::get<0>(cogFrame) = cogX - cogHalfEdge - 1;
        std::get<1>(cogFrame) = cogY - cogHalfEdge - 1;
        std::get<2>(cogFrame) = cogX + cogHalfEdge + 1;
        std::get<3>(cogFrame) = cogY + cogHalfEdge + 1;
  
      
        // HFD calculation --> In: image, Out: HFD value
        // Subtract mean value from image which is required for HFD calculation
        size_t hfdRectDist = floor(outerHfdDiameter / 2.0);
        std::get<0>(hfdFrame) = cogX - hfdRectDist;
        std::get<1>(hfdFrame) = cogY - hfdRectDist;
        std::get<2>(hfdFrame) = cogX + hfdRectDist;
        std::get<3>(hfdFrame) = cogY + hfdRectDist;
    
        CImg<float> hfdSubImg = aiImg.get_crop(std::get<0>(hfdFrame), std::get<1>(hfdFrame), std::get<2>(hfdFrame), std::get<3>(hfdFrame));
        maxPixValue = hfdSubImg.max();
        saturated = (maxPixValue == maxPossiblePixValue);
        
        CImg<float> imgHfdSubMean(hfdSubImg);
        float & mean = it->mean;

		int w = std::get<2>(frame) - std::get<0>(frame);
		if (w > max_w) {
			max_w = w;
			maxWIndex = i;
		}

        i++;

        cimg_forXY(hfdSubImg, x, y) {
          imgHfdSubMean(x, y) = (hfdSubImg(x, y) < mean ? 0 : hfdSubImg(x, y) - mean);
        }
  
        // Calc the HFD
        hfd = calcHfd(imgHfdSubMean, outerHfdDiameter /*outer diameter in px*/);
    }

    i = 0;

    // Mark all stars in RGB image
    for (StarInfoListT::iterator it = starInfos.begin(); it != starInfos.end(); ++it) {
        StarInfoT * curStarInfo = & (*it);
        PixSubPosT & cogCentroid = curStarInfo->cogCentroid;
        float & hfd = curStarInfo->hfd;
        float & fwhmHorz = curStarInfo->fwhmHorz;
        float & fwhmVert = curStarInfo->fwhmVert;
        float & maxPixValue = curStarInfo->maxPixValue;
        
        if (i == maxWIndex) {
        //   log_i("cogCentroid = (%f, %f), hfd: %f, mean: %f", std::get<0>(curStarInfo->cogCentroid), std::get<1>(curStarInfo->cogCentroid), hfd, it->mean);
			result.maxHfd = hfd;
			const FrameT & frame = curStarInfo->clusterFrame;
			FrameT squareFrame(rectify(frame));
			result.maxFrame = squareFrame;
			result.maxW = std::get<2>(squareFrame) - std::get<0>(squareFrame);
			// CImg<float> img = aiImg.get_crop(std::get<0>(squareFrame), std::get<1>(squareFrame), std::get<2>(squareFrame), std::get<3>(squareFrame));
        }

        i++;
    }
  
  	return result;
}